// all angle related state is replanced by speed,so name vechile_lateral_speed.h
#pragma once
#include <iostream>
#include <Eigen/Dense>
#include <memory>
#include "vehicle_dyn_base.hpp"

namespace auto_ros
{
namespace control
{
class VehicleLateralSpeed : public VehicleDynBase
{
public:
	//init state fun when speed is 1
	VehicleLateralSpeed(std::string dyn_yaml_path_name);
	virtual void para_update_matrix(double speed);
	double mass();
	double lf();
	double lr();
	double cf();
	double cr();
	double iz();
	double max_tire_f_angle();
	double speed_threshold();
	bool set_mass(double mass);
	bool set_lf(double lf);
	bool set_lr(double lr);
	bool set_cf(double cf);
	bool set_cr(double cr);
	bool set_iz(double iz);
	bool set_max_lateral_acc(double max_acc);
	bool set_max_tire_f_angle(double max_angle);

private:
	//paramter
	double mass_;
	double cf_;
	double cr_;
	double lf_;
	double lr_;
	double iz_;
	double h_;
	//local parameter
	double c_yaw_rate_;
	double c_beta_;
	double c_delta_;
	double d_yaw_rate_;
	double d_delta_;
	double d_beta_;
	//steady state and control variable
	Eigen::VectorXd steady_state_;
	Eigen::VectorXd steady_control_;
	double max_lateral_acc_;
	double max_tire_f_angle_;
	//state
	double speed_;
	double speed_threshold_; // divided by 0 error
};
} // namespace control
} // namespace auto_ros